/**
 * ...
 * @author Krzysztof Rozalski
 */

package hapi.physics.integration;
import hapi.math.Vector2D;
import hapi.physics.Acceleration;
import hapi.physics.AngularAcceleration;
import hapi.physics.AngularVelocity;
import hapi.physics.Time;
import hapi.physics.Velocity;

class RK4Integration implements NumericalIntegrationStrategy {

	var accelerator:Accelerator;
	
	public function new(accelerator:Accelerator) {
		this.accelerator = accelerator;
	}
	
	public function integrate(target:Integrateable, time:Time, delta:Time):Integrateable {
		var halfDelta:Time = new Time(delta.seconds * 0.5);
		
		var a:Derivative = evaluateWithoutDerivative(target, time);
		var b:Derivative = evaluate(target, time, halfDelta, a);
		var c:Derivative = evaluate(target, time, halfDelta, b);
		var d:Derivative = evaluate(target, time, delta, c);
		
		//TODO refactor - make derivative additive and multiplicative
		var velocity:Velocity = calcTaylorVelocity(a.velocity, b.velocity, c.velocity, d.velocity);
		var acceleration:Acceleration = calcTaylorAcceleration(a.acceleration, b.acceleration, c.acceleration, d.acceleration);
		var angularVelocity:AngularVelocity = calcTaylorAngularVelocity(a.angularVelocity, b.angularVelocity, c.angularVelocity, d.angularVelocity);
		var angularAcceleration:AngularAcceleration = calcTaylorAngularAcceleration(a.angularAcceleration, b.angularAcceleration, c.angularAcceleration, d.angularAcceleration);
		
		velocity.applyTo(target.position, delta);
		acceleration.applyTo(target.velocity, delta);
		angularVelocity.applyTo(target.angle, delta);
		angularAcceleration.applyTo(target.angularVelocity, delta);
		return target;
	}
	
	inline function evaluateWithoutDerivative(initial:Integrateable, time:Time):Derivative {
		return new Derivative(initial, time, accelerator);
	}

	function evaluate(target:Integrateable, time:Time, delta:Time, d:Derivative):Derivative {
		var newTarget = target.derivative(d, delta);
		return new Derivative(newTarget, new Time(time.seconds+delta.seconds), accelerator);
	}
	
	inline function calcTaylorVelocity( a:Velocity, b:Velocity, c:Velocity, d:Velocity ):Velocity {
		var twoTimesBPlusC = b.add(c).multiplyBy(2);
		var v = a.add( twoTimesBPlusC ).add( d );
		return v.multiplyBy(1/6);
	}
	
	inline function calcTaylorAcceleration( a:Acceleration, b:Acceleration, c:Acceleration, d:Acceleration ):Acceleration {
		var twoTimesBPlusC = b.add(c).multiplyBy(2);
		var v = a.add( twoTimesBPlusC ).add( d );
		return v.multiplyBy(1/6);
	}
	
	inline function calcTaylorAngularVelocity( a:AngularVelocity, b:AngularVelocity, c:AngularVelocity, d:AngularVelocity ):AngularVelocity {
		var twoTimesBPlusC = b.add(c).multiplyBy(2);
		var v = a.add( twoTimesBPlusC ).add( d );
		return v.multiplyBy(1/6);
	}
	
	inline function calcTaylorAngularAcceleration( a:AngularAcceleration, b:AngularAcceleration, c:AngularAcceleration, d:AngularAcceleration ):AngularAcceleration {
		var twoTimesBPlusC = b.add(c).multiplyBy(2);
		var v = a.add( twoTimesBPlusC ).add( d );
		return v.multiplyBy(1/6);
	}
}